\section{Software Architecture and Implementation}
Detailed in this section is the method and reason to our software architecture. In particular, we aimed for our architecture to be stateless with respect to the current operation, to maximize efficiency of joystick checks (which are normally slow in RobotC), and to allow dynamic and easy reassignment of both controller information during teleop, as well as both movement sequencing and heuristic decision trees during autonomous.

We have chosen a particular structure for our code. We have rewritten \lstinline{JoystickDriver.c}{} to better express our needs. By removing superfluous tasks from the driver, we maintained functionality while increasing our efficiency by approximately threefold. We have created a set of utility headers: \lstinline{teleoputils.h}{}, \lstinline{autoutils.h}{}, and \lstinline{sharedutils.h}{} to address the need for macros, \lstinline{#define}{} statements, and standardized functions. Both \lstinline{teleoputils.h}{} and \lstinline{autoutils.h}{} import \lstinline{sharedutils.h}{}, so that file is never explicitly included in the main code body. Drivers were pulled from HiTechnic's online library for 3rd party sensors.

Additionally, it may be worth taking a peek into the code appendices in the back of this engineering notebook.

\subsection{Teleoperated Mode}

Teleoperated mode has the following requirements: \begin{enumerate}
	\item{Smooth arcade driving}
	\item{Easy reassignment of buttons}
	\item{Stateless control of motors and robot state}
	\item{Efficient button control and loop checking}
	\item{\textbf{Must} use only one controller}
\end{enumerate}

In order to implement this effectively, we have implemented several macros. These macros allow us to later set the powers of the motors without much effort. 

\subsubsection{Drive Code and Reasoning}

Our main body of code is run through the following:

\begin{lstlisting}[tabsize=4]
task main() {
	while(true) {
		getJoystickSettings(joystick);
		checkJoystickButtons();
		setLeftMotors(powscl(JOY_Y1)-powscl(JOY_X1)/1.75);
		setRightMotors(powscl(JOY_Y1)+powscl(JOY_X1)/1.75);
	}
}
\end{lstlisting}

First, grab the current joystick configuration from the controllers. Then, check to see if any buttons have changed (\lstinline{checkJoystickButtons();}{}). Then, set the motor powers for arcade drive. 

\paragraph{Power scaling} The \lstinline{powscl(int)}{} function's definition is intended to compensate for the large deadband range which occurs under standard drive conditions. The controller's user really only needs two ranges: a high-precision, low power range near zero, and a low-precision, high power range near the maxima. While an exponential function could be used, it is much slower, and much more hard to tune. Instead, we draw two lines: a shallow slope for the first segment, then a large slope for the second segment of the controller. This provides both high precision and high power where needed. As the driver does not generally use the range in $[60,85]\%$, there is no concern about the nonlinearity. The function is defined as follows:

\begin{lstlisting}[tabsize=4]
float powscl(int xz) {
	float sign = (float)sgn(xz);
	float x = abs(xz)/128.0;
	if(x < DISTA)
		return 100* sign * (x*SLOPE);
	else
		return 100* sign * ((DISTA*SLOPE*(x-1.0) - x + DISTA) / (DISTA - 1.0));
}
\end{lstlisting}

\paragraph{Compensation for the Old and New Joystick Configuration} It is necessary to compensate for both the old and new controller configurations. As the controller has been updated, the buttons have changed - however, competition rules permit the use of both controllers. Therefore, we must be able to accommodate this change if necessary. We have done so through the use of a define statement: if we have \lstinline{#define ALTLOG}{}, then we switch to the old button layout.

\paragraph{Button Press Checking Requires a Stateless Organization} In order to easily and effectively change the functionality of the controller, a particular design was implemented:

\begin{lstlisting}[tabsize=4]
void invokeButton(int button, bool pressed) {
	switch(button) {
		case JOY_X:  if(pressed) {servo[servoL1] = 156; servo[servoL2] = 26;} else {} break;
		case JOY_Y:  if(pressed) {servo[servoL1] = 120; servo[servoL2] = 40;} else {} break;
		case JOY_A:  if(pressed) {} else {} break;
		case JOY_B:  if(pressed) {motor[mSpin] = 100;} else {motor[mSpin] = 0;} break;
		case JOY_RB: if(pressed) {setArmMotors(100);}  else {setArmMotors(0);} break;
		case JOY_LB: if(pressed) {setArmMotors(-100);} else {setArmMotors(0);} break;
		case JOY_R3: if(pressed) {} else {} break;
		case JOY_L3: if(pressed) {} else {} break;
	}
}

bool t[8];
void checkJoystickButtons() {
	for(int i = 0; i < 8; i++) {
		if(joy1Btn(i) != t[i]) {
			invokeButton(i, !t[i]);
			t[i] = !t[i]; 
		}
	}
}
\end{lstlisting}

This may appear confusing at first, however, there are a couple points: \lstinline{checkJoystickButtons()}{} is actually called from the main loop. It simply checks to see what buttons have changed on the controller, and calls the appropriate \lstinline{invokeButton(int, bool)}{} arguments. In doing so, we can determine exact behavior on button presses with ease. As our robot is very simple, we do not need more than a handful of buttons, so most of them remain unassigned.

\newpage \paragraph{Code Optimizations} Although this method works for determining which button is being pressed, we quickly found it is not the most elegant way of doing so. First off, we use a \lstinline{bool[]}{} to hold the state of each button. This seems intuitive, but the RobotC compiler actually creates an entire \lstinline{char}{} to hold either \lstinline{true}{} or \lstinline{false}{} for every \lstinline{bool}{}; this is a waste of memory. The other issue can be found in the processing structure. Each iteration of the program checks all possible button conditions; this is a waste of processing time if there has been no change since the last iteration.

\begin{lstlisting}[tabsize=4]
short btn = JOY_BTN;//local store = live store, initially
void checkJoystickButtons() {
	if(btn == JOY_BTN) return;//checksum
	for(short i = 11; i >= 0; i--) {
		if((btn>>i) ^ (JOY_BTN>>i)) {//check each button for a change
			invokeButton(i, ((btn & (1 << i)) == 0));//trigger event (#, down|up)
			btn ^= 1<<i;//mirror changes in local store
		}
	}
}
\end{lstlisting}

We solve the former issue by means of storing each button state in a single bit of data, reducing our memory footprint. Teams are encouraged to call the \lstinline{joy1Btn(int)}{} function each time they wish to check the state of a particular button, but this can become cumbersome of one wishes check multiple buttons in real-time. The ``JoystickDriver.c'' file that we must use for field communications stores each button state in a single \lstinline{short}{}; this means that we are capable of running a checksum of the joystick state before we check each button. This not only reduces our time per iteration, but also allows for our robot to be more responsive to joystick changes due to the inherent speed of bitwise operations.

\newpage \subsection{Autonomous Mode}
Autonomous, much like teleop, must meet certain criteria: \begin{enumerate}
	\item{Recognize which crate has been chosen with the IR beacon}
	\item{Move to said crate in a timely manor}
	\item{Move to onto the bridge after scoring our block}
	\item{Avoid any other robots on the way}
\end{enumerate}

\paragraph{Crate Detection} Much like any other team, we use the HiTechnic IR Seeker to determine which crate our robot should pursue. We use a function that reads not only the position of the IR beacon relative to our location on the field, but also the strength at which it is reading. Having more than one variable to check greatly reduces the generally unavoidable environment-error the comes hand-in-hand with Infrared Light detection.

\paragraph{Movement Functions} Using motor encoders, sensing how far a robot has moved is relatively simple in theory. Generally, the code becomes messy when the programmer has to remember motor encoder values and direction. To solve this issue, we have implemented general movement functions based on inches.
\begin{lstlisting}[tabsize=4]
void rbtMoveFdTime(float inches, int msec) {
	int enc = getEncoderByInches(inches); clearEncoders();
	int norm = -1.0*sgn(inches);
	ClearTimer(DrTimer);
	while(leftEncoder < enc && rightEncoder < enc && time1[DrTimer] < msec) {
		setLeftMotors (100*norm);
		setRightMotors(100*norm);
	}
	setLeftMotors(0); setRightMotors(0);
}
\end{lstlisting}
This function allows our robot to move forward by a arbitrary number if inches and finish the motion in less than the specified time. Generally one does not want to move forward for an amount of time because the power fluctuates with battery levels. On the same hand, if one moves forwards based on just encoder values, the motor have the potential to burn out if the robot incurs a collision. Allowing the function to reach the specified distance before a certain amount of time insures that neither of these situations have a high probability of surfacing.

\paragraph{Object Avoidance} Through the use of the HiTechnic SuperPro that was made legal by this year's game manual, we have been able to mount a high performance ultrasonic sensor to validate that the path we wish to take is open. If there is an obstruction, the robot moves onto the bridge via an alternate route. This feature increases the probability of a higher average score.

\begin{lstlisting}[tabsize=4]
bool pathClear(float dist){
	pause();
	float read = 0;
	for(int i=0;i<10;i++){read+=(analogRead(A3)*0.4);wait1Msec(5);}
	nxtDisplayBigTextLine(3,"%f", read/10.0);
	wait1Msec(2000);
	return ((read/10)<dist?false:true);
}
\end{lstlisting}

\newpage \subsection{Algorithms and Cartesian Mathematics}

\subsubsection{Hybrid Localization Using Gyroscopes \& Odometers}
\paragraph{Odometric Data}
We begin by assigning the following constants: 
\[D_{ot}=\frac{\text{Distance}}{\text{odometer tick}}=\pi(\text{wheel diameter})/(\text{ticks/revolution})\]\[ \theta_{ot} = \frac{\theta}{\text{odometer tick}} = \pi\left(\frac{\text{wheel diameter}}{\text{distance between wheels}}\right)/(\text{ticks/revolution})\]
We can calculate $(x_{\mathrm{enc}}, y_{\mathrm{enc}}, \theta_{\mathrm{enc}})$ from the odometer as follows: 
\begin{center}
	\begin{align*}
		\mathrm{d}l &= l^t_{\mathrm{enc}}-l^{t-1}_{\mathrm{enc}} \\
		\mathrm{d}r &= r^t_{\mathrm{enc}}-r^{t-1}_{\mathrm{enc}} \\
		\mathrm{d}D &= \frac{1}{2}(\mathrm{d}l+\mathrm{d}r)D_{ot} \\
		\mathrm{d}x_{\mathrm{enc}} &= \mathrm{d}D\cos(\theta^t_{enc}) \\
		\mathrm{d}y_{\mathrm{enc}} &= \mathrm{d}D\sin(\theta^t_{enc}) \\
		\mathrm{d}\theta_{\mathrm{enc}} &= (\mathrm{d}r-\mathrm{d}l)\theta_{ot} \\
		x_{\mathrm{enc}} &= x^{t-1}_{\mathrm{enc}} + \mathrm{d}x_{\mathrm{enc}} \\
		y_{\mathrm{enc}} &= y^{t-1}_{\mathrm{enc}} + \mathrm{d}y{\mathrm{enc}} \\
		\theta_{\mathrm{enc}} &= \theta^{t-1}_{\mathrm{enc}} + \mathrm{d}\theta_{\mathrm{enc}}
	\end{align*}
\end{center}

$l$ denotes the left side of the robot, and $r$ denotes the right side of the robot. $D$ denotes the distance. 

\paragraph{Localization Algorithm}
The robots motor controller calculates position and orientation $(x_{\mathrm{enc}}, y_{\mathrm{enc}}, \theta_{\mathrm{enc}})$ from encoder ticks and sends the data to an on-board computer. The mounted gyroscope communicates with a gyro driver which integrates the rate values into an absolute angle ($\theta_{\mathrm{gyro}}$). Global position $(x_{\mathrm{rbt}}, y_{\mathrm{rbt}})$ is found by transforming the translation vector from encoder space to gyroscope space. Global angle $(\theta_{\mathrm{rbt}})$ is the gyro angle ($\theta_{\mathrm{gyro}}$). The following describes the computation as an iterative algorithm: 

\begin{center}
	\begin{align*}
		\mathrm{d}x&=x^{t}_{\mathrm{enc}}-x^{t-1}_{\mathrm{enc}} \\
		\mathrm{d}y&=y^{t}_{\mathrm{enc}}-y^{t-1}_{\mathrm{enc}} \\
		\mathrm{d}\theta&=\theta^t_{\mathrm{gyro}}-\theta^{t}_{\mathrm{enc}} \\
		x^t_{\mathrm{rbt}}&=x^{t-1}_{\mathrm{rbt}}+\cos(\mathrm{d}\theta)\mathrm{d}x-\sin(\mathrm{d}\theta)\mathrm{d}y \\
		y^t_{\mathrm{rbt}}&=y^{t-1}_{\mathrm{rbt}}+\sin(\mathrm{d}\theta)\mathrm{d}x+\cos(\mathrm{d}\theta)\mathrm{d}y \\
		\theta^t_{\mathrm{rbt}}&=\theta^t_{\mathrm{gyro}}
	\end{align*}
\end{center}
